#ifndef __INFLATION_LAYER_H__
#define __INFLATION_LAYER_H__

#include "common/layered_costmap.h"

namespace costmap_2d{

class CellData{
public:
    CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
        index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy){}
    unsigned int index_;
    unsigned int x_, y_;
    unsigned int src_x_, src_y_;
};

class InflationLayer : public Layer, public Costmap2D{
    public:
        InflationLayer();
        virtual ~InflationLayer();

        virtual void onInitialize();
        virtual void matchSize();
        virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
            double* max_x, double* max_y);
        virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

        void setMap(const nav_msgs::OccupancyGridConstPtr& new_map) {};
        void createBuffer(boost::shared_ptr<costmap_2d::ObservationBuffer> buffer, string topic_name){};
        void addPointCloud2(sensor_msgs::PointCloud2 cloud, string topic_name) {};

        unsigned char computeCost(double distance) const;
        double distanceLookup(int mx, int my, int src_x, int src_y);
        unsigned char costLookup(int mx, int my, int src_x, int src_y);
        unsigned int cellDistance(double world_dist){
            return layered_costmap_->getCostmap2D()->cellDistance(world_dist);
        }
        void computeCaches();
        void enqueue(unsigned int index, unsigned int mx, unsigned int my,
            unsigned int src_x, unsigned int src_y);
        void deleteKernels();
        void setInflationParameters(double inflation_radius, double cost_scaling_factor);


    private:
        double inflation_radius_;
        double inscribed_radius_;
        double resolution_;
        double weight_;
        unsigned int cell_inflation_radius_;
        unsigned int cached_cell_inflation_radius_;
        std::map<double, std::vector<CellData> > inflation_cells_;
    
        bool* seen_;
        int seen_size_;
        unsigned char** cached_costs_;
        double** cached_distances_;
        bool need_reinflation_;
        bool inflate_unknown_;
        double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
};


};   // namespace costmap_2d

#endif